// --------------------------------------------------------------------------------------------------------------------
// <copyright file="CameraPoseEstimationModelAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Visualization
{  
    using System;
    using System.ComponentModel;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    using Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction;
    using Microsoft.Robotics.Visualization;

    /// <summary>
    /// This model agent class prepares the  camera pose model for visualization. 
    /// It also propagates updates to the landmark id via UI to the CameraPoseEstimationAgent.
    /// </summary>
    [DataContract]
    public sealed class CameraPoseEstimationModelAgent : ModelAgent<RGBCameraAgentMessage, CameraPoseEstimationModel>,
        ISubscriptionReceiver<CameraPoseEstimationAgentMessage>
    {
        /// <summary>
        /// Name of the agent producing the camera pose estimation agent message to subscribe to
        /// </summary>
        [DataMember(Name = "CameraPoseEstimationMessageProducer")]
        private string cameraPoseEstimationMessageProducer;

        /// <summary>
        /// Select the type of display
        /// </summary>
        [DataMember(Name = "ImageFormat")]
        private SimplePixelFormat? imageFormat;

        /// <summary>
        /// CameraPoseEstimationMessageProducerAgent we are subscribed to.
        /// </summary>
        private IAgentHost cameraPoseEstimationMessageProducerAgent;

        /// <summary>
        /// Camera Pose Estimation Input Forwarder used to pass the UI updates to the camera pose estimation agent.
        /// </summary>
        private IForwarder<CameraPoseEstimationInputMessage> cameraPoseEstimationInputForwarder;

        /// <summary>
        /// Previous Landmark Id which was posted to the camera pose estimation input forwarder port.
        /// </summary>
        private int? previousLandmarkId;

        /// <summary>
        /// Initializes a new instance of the CameraPoseEstimationModelAgent class        
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="rgbImageProducer">The RGB image producer.</param>
        /// <param name="cameraPoseEstimationProducer">The camera pose estimation producer.</param>
        /// <param name="maxUpdateIntervalMilliseconds">The interval on which to issue data binding updates to the UI</param>
        /// <param name="imageFormat">Image format</param>
        public CameraPoseEstimationModelAgent(
            string name,
            IProducer<RGBCameraAgentMessage> rgbImageProducer,
            IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimationProducer,
            int maxUpdateIntervalMilliseconds,
            SimplePixelFormat imageFormat)
            : base(name, rgbImageProducer.Name, maxUpdateIntervalMilliseconds)
        {
            this.imageFormat = (SimplePixelFormat)imageFormat;
            this.cameraPoseEstimationMessageProducer = cameraPoseEstimationProducer.Name;
        }

        /// <summary>
        /// Initializes any fields not provided in the manifest.
        /// </summary>
        /// <param name="locator">Not used.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
         
            if (this.imageFormat == null)
            {
                this.imageFormat = SimplePixelFormat.Gray8;
            }

            if (this.cameraPoseEstimationMessageProducer != default(string))
            {
                this.cameraPoseEstimationMessageProducerAgent = this.Locator.GetAgent(this.cameraPoseEstimationMessageProducer);
                this.cameraPoseEstimationMessageProducerAgent.Subscribe<CameraPoseEstimationAgentMessage>(this);
            }

            // Set the default message to be camera not calibrated. If there is a ground truth message, that will overwrite this value.
            this.Model.PlanarPose = "Camera not calibrated";

            this.Model.PropertyChanged += new System.ComponentModel.PropertyChangedEventHandler(this.ModelPropertyChanged);
            this.cameraPoseEstimationInputForwarder  = this.cameraPoseEstimationMessageProducerAgent.GetControlPort<CameraPoseEstimationInputMessage>();
            this.previousLandmarkId = null;
        }

        /// <summary>
        /// Process camera pose estimation message and update the UI
        /// </summary>
        /// <param name="message">Camera pose estimation message</param>
        public void Receive(CameraPoseEstimationAgentMessage message)
        {
            this.Model.PlanarPose = string.Format(
                "Planar X = {0}, Y = {1}, Z = {2} AX = {3}, AY = {4}, AZ = {5}",
                (message.Translations.X * MathConstants.MilliunitsPerUnit).ToString("F2"),
                (message.Translations.Y * MathConstants.MilliunitsPerUnit).ToString("F2"),
                (message.Translations.Z * MathConstants.MilliunitsPerUnit).ToString("F2"),
                (message.Rotations.X * MathConstants.Radians2Degrees).ToString("F2"),
                (message.Rotations.Y * MathConstants.Radians2Degrees).ToString("F2"),
                (message.Rotations.Z * MathConstants.Radians2Degrees).ToString("F2"));

            string cameraX = string.Empty;
            string cameraY = string.Empty;
            string cameraHeading = string.Empty;

            if (message.CameraPose != null)
            {
                cameraX = (message.CameraPose.Value.X * MathConstants.MilliunitsPerUnit).ToString("F2");
                cameraY = (message.CameraPose.Value.Y * MathConstants.MilliunitsPerUnit).ToString("F2");
                cameraHeading = (message.CameraPose.Value.Heading * MathConstants.Radians2Degrees).ToString("F2");
            }

            this.Model.CameraPose = string.Format(
                "Camera X = {0}, Y = {1}, Th = {2}",
                cameraX,
                cameraY,
                cameraHeading);

            string landmarkId = string.Empty;
            string landmarkX = string.Empty;
            string landmarkY = string.Empty;
            string landmarkHeading = string.Empty;

            if (message.Landmark != null)
        {
                landmarkId = (message.Landmark.Id).ToString("F2");
                landmarkX = (message.Landmark.Pose.X * MathConstants.MilliunitsPerUnit).ToString("F2");
                landmarkY = (message.Landmark.Pose.Y * MathConstants.MilliunitsPerUnit).ToString("F2");
                landmarkHeading = (message.Landmark.Pose.Heading * MathConstants.Radians2Degrees).ToString("F2");
            }

            this.Model.Landmark =
                string.Format(
                "Landmark Id = {0} X = {1} Y = {2} Heading = {3}",
                landmarkId,
                landmarkX,
                landmarkY,
                landmarkHeading);
        }

        /// <summary>
        /// Updates the model data with the new state.
        /// </summary>
        /// <param name="state">The new state from the upstream agent.</param>
        /// <returns>true if the model was updated, false otherwise</returns>
        protected unsafe override bool UpdateModel(RGBCameraAgentMessage state)
        {
            ImageFrameGray8 grayImage = ImageOperations2DInterop.ConvertYUY2ToGray8Image(state.RGBImageFrame);

            System.Drawing.Size lumSize = new System.Drawing.Size(state.RGBImageFrame.Width, state.RGBImageFrame.Height);            

            if (this.imageFormat == SimplePixelFormat.Gray8)
            {
                this.Model.VideoFrame =
                new VideoFrame()
                {
                     Data = grayImage.ImageData,
                    Size = lumSize,
                    Format = SimplePixelFormat.Gray8,
                     Stride = grayImage.Width * grayImage.BytesPerPixel
                };
            }
            else
            {
                throw new Exception("RGBCamera - unsupported image type " + this.imageFormat.ToString());
            }

            return true;
        }

        /// <summary>
        /// Processes any changes to the model property and forwards them to the camera estimation agent's input port.
        /// </summary>
        /// <param name="sender">Event sender</param>
        /// <param name="eventArgs">Event Arguments</param>
        private void ModelPropertyChanged(object sender, PropertyChangedEventArgs eventArgs)
        {
            if (this.previousLandmarkId == null || this.previousLandmarkId.Value != this.Model.LandmarkId)
            {
                this.cameraPoseEstimationInputForwarder.Post(new CameraPoseEstimationInputMessage(this.Model.LandmarkId));
                this.previousLandmarkId = this.Model.LandmarkId;
            }
        }
    }
}
